#include "headers/CV.hpp"
#include "headers/all_control_define.hpp"
#include <cmath>

Mat element3X3 = getStructuringElement(MORPH_ELLIPSE, Size(3, 3)),
element5X5 = getStructuringElement(MORPH_ELLIPSE, Size(5, 5)),
element7X7 = getStructuringElement(MORPH_ELLIPSE, Size(7, 7)),
element9X9 = getStructuringElement(MORPH_ELLIPSE, Size(9, 9)),
element11X11 = getStructuringElement(MORPH_ELLIPSE, Size(11, 11)),
element13X13 = getStructuringElement(MORPH_ELLIPSE, Size(13, 13));

Mat elementNXN_OPEN1 = getStructuringElement(MORPH_ELLIPSE, Size(KERNEL_SIZE_OPEN1, KERNEL_SIZE_OPEN1))
, elementNXN_CLOSE1 = getStructuringElement(MORPH_ELLIPSE, Size(KERNEL_SIZE_CLOSE1, KERNEL_SIZE_CLOSE1));

string program_root_path;

bool Pre_proc(AccMat& input_frame, AccMat& output_bin) {
	if(input_frame.empty())return false;

	Scalar top_color(THRESH_COLOR_UPPER), below_color(THRESH_COLOR_LOWER);
	
#if CV_DEBUG
	static int is_created = 0, thr_top = top_color[0], thr_below = below_color[0];
	if(is_created == 0){
		namedWindow("thr_bar");
		createTrackbar("thresh_t", "thr_bar", &thr_top, 255);
		createTrackbar("thresh_b", "thr_bar", &thr_below, 255);
		is_created = 1;
	}

	top_color[0] = thr_top;
	below_color[0] = thr_below;
#endif

	AccMat TempAccMat1, TempAccMat2, TempAccMat3;
	Mat TempMat, splited_frame[3], sub_result_img, h, v;
	
	cvtColor(input_frame, TempMat, CV_BGR2HSV);
	
	split(TempMat, splited_frame);

	//h = splited_frame[0];
	v = splited_frame[2];

	v.copyTo(TempAccMat1);
	//cv::erode(TempAccMat1, TempAccMat2, element3X3);
	//cv::dilate(TempAccMat2, TempAccMat3, element3X3);
	cv::blur(TempAccMat1, TempAccMat2, Size(5, 5));
	cv::normalize(TempAccMat2, TempAccMat1, 0, 255, NORM_MINMAX);

	//cout << 1 << endl;

	//cv::adaptiveThreshold(TempAccMat1, TempAccMat2, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 7, (below_color[0] - top_color[0]));

	cv::inRange(
		TempAccMat1,
		below_color,
		top_color,
		TempAccMat2
	);

	//output_bin.copyTo(TempAccMat);

	erode(TempAccMat2, TempAccMat1, elementNXN_OPEN1, Point(-1,-1), CALC_TIMES_OPEN1);
	dilate(TempAccMat1, TempAccMat2, elementNXN_OPEN1, Point(-1,-1), CALC_TIMES_OPEN1);

	dilate(TempAccMat2, TempAccMat1, elementNXN_CLOSE1, Point(-1,-1), CALC_TIMES_CLOSE1);
	erode(TempAccMat1, output_bin, elementNXN_CLOSE1, Point(-1,-1), CALC_TIMES_CLOSE1);

#if CV_DEBUG
	show_image("bin image", output_bin, 500);
#endif

	return true;
}

void arrange_rrect_points(Point2f* rrect_src, Point2f* rrect_dst) {
	static Point2f pi, pj, pt;
	static float r_i, r_j, t;

	pi = rrect_src[1] - rrect_src[0];
	pj = rrect_src[3] - rrect_src[0];
	r_i = std::abs(pi.x) + std::abs(pi.y);
	r_j = std::abs(pj.x) + std::abs(pj.y);

	if (r_j > r_i) {
		pt = rrect_src[1];
		rrect_dst[1] = rrect_src[3];
		rrect_dst[3] = pt;
	}
}

/*bool get_item_feature(Mat& Input_image, Mat& out_feature) {
	static Rect temp_roi, move_roi_rect;
	static Point move_vec;
	static Mat cutted_roi, temp_mat;
	static int i;
	static float sum_points_num;

	if(out_feature.empty())out_feature.create(FEATURE_ROWS, FEATURE_COLS, CV_32FC1);
	temp_roi = Rect(0, 0, Input_image.cols / FEATURE_COLS, Input_image.rows / FEATURE_ROWS);
	temp_mat = Input_image / 255;
	sum_points_num = cv::sum(temp_mat)[0];

	for (i = 0; i < FEATURE_SIZE; i++) {
		move_vec.x = temp_roi.width * (i % FEATURE_COLS);
		move_vec.y = temp_roi.height * (i / FEATURE_COLS);
		move_roi_rect = temp_roi + move_vec;

		Input_image(move_roi_rect).copyTo(cutted_roi);
		temp_mat = cutted_roi / 255;

		out_feature.at<float>(i) = (float)(cv::sum(temp_mat)[0]) / (float)sum_points_num;
	}

	return true;

}*/

//old method
/*
bool get_item_feature_AccMat(AccMat& Input_image, Mat& out_feature, vector<Point> &contour) {
	out_feature.create(1,3,CV_32F);

	AccMat TempAccMat1;
	Mat TempMat;

	vector<Vec4i> hierarchy;

	double feature_sum_val;
	int cols = Input_image.cols, rows = Input_image.rows;

	double ellipse_area = Input_image.cols * Input_image.rows, IN_contourArea = 0, temp_d;
	
	IN_contourArea = contourArea(contour);

	(out_feature.at<Vec<float,1> >(0))[0] = float(IN_contourArea/ellipse_area);

	Rect cut_ROI1(Point(0,0), Point(Input_image.cols/2, Input_image.rows-1)), 
	cut_ROI2(Point(Input_image.cols - Input_image.cols/2 + 1, 0), Point(Input_image.cols-1, Input_image.rows-1)),
	cut_ROI3(Point(Input_image.cols*3/8, 0), Point(Input_image.cols*5/8, Input_image.rows-1));

	double sum_roi1=0, sum_roi2=0, sum_roi3=0;

	if(cut_ROI1.area() > 0){
		sum_roi1 = cv::sum(Input_image(cut_ROI1))[0] / 255.0;
	}

	if(cut_ROI2.area() > 0){
		sum_roi2 = cv::sum(Input_image(cut_ROI2))[0] / 255.0;
	}


	(out_feature.at<Vec<float,1> >(1))[0] = (float)sum_roi1;
	(out_feature.at<Vec<float,1> >(2))[0] = (float)sum_roi2;

	return true;

}
*/

bool get_item_feature_AccMat(AccMat& Input_image, Mat& out_feature, vector<Point> &contour){
	AccMat TempAccMat1, TempAccMat2;
	Mat TempMat;
	Input_image.convertTo(TempAccMat1, CV_32F);

	resize(TempAccMat1, TempAccMat2, Size(32,32));
	cv::dct(TempAccMat2, TempAccMat1);

	Rect ROI_cut_rect(Point(0,0), Point(8,8));

	TempAccMat1(ROI_cut_rect).copyTo(TempMat);

	double avg = cv::sum(TempMat)[0]/64.0;

	threshold(TempMat, out_feature, avg, 1, CV_THRESH_BINARY);

	//cout << "out_feature.rows: " << out_feature.rows << "out_feature.cols: " << out_feature.cols << endl;

	return true;
}

bool get_item_feature(Mat& Input_image, Mat& out_feature) {
	AccMat In_AccMat = AccMat();
	vector<vector<Point> > contours;
	vector<Point> contour;
	double max_area, temp_area;

	findContours(Input_image, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
	
	max_area = 0;
	for(auto t_contour : contours){
		temp_area = contourArea(t_contour);
		if(max_area < temp_area){
			max_area = temp_area;
			contour = t_contour;
		}
	}
	
	Input_image.copyTo(In_AccMat);
	get_item_feature_AccMat(In_AccMat, out_feature, contour);

	return true;

}

double calc_err(Mat &item_feature, Mat &template_feature){
	Mat diff_feature;
	float feature_num = float(item_feature.cols * item_feature.rows);

	absdiff(item_feature, template_feature, diff_feature);

	int i;
	float out_err = 0;
	for(i=0;i<feature_num;i++){
		out_err += diff_feature.at<Vec<float, 1> >(i)[0] / (template_feature.at<Vec<float, 1> >(i)[0] + 0.0001f);
	}
	out_err /= feature_num;

	return double(out_err);
}

double calc_err_H(Mat &item_feature, Mat &template_feature){
	Mat TempMat = item_feature != template_feature;
	//cout << "TempMat: " << TempMat << endl;
	//waitKey();
	double HammingDistance = cv::sum(TempMat)[0]/255.0;

	return HammingDistance / item_feature.cols / item_feature.rows;
}

double find_ammor_to_arm_dri_vec(RotatedRect& ammor_rrect, Point2f& arm_center, Point2f& dri_vec){
	static Point2f ammor_rrect_points[4], temp_vec1, temp_vec2;
	ammor_rrect.points(ammor_rrect_points);
	static double cur_in_product, max_in_product;
	static int i;
	for(i=0;i<4;i++){
		temp_vec1 = (ammor_rrect_points[i] + ammor_rrect_points[(i+1)%4]) / 2.0 - ammor_rrect.center;
		temp_vec2 = arm_center - ammor_rrect.center;
		cur_in_product = temp_vec1.ddot(temp_vec2);

		if(i==0 || cur_in_product > max_in_product){
			max_in_product = cur_in_product;
			dri_vec = temp_vec1;
		}
	}
	
	return std::sqrt(dri_vec.x*dri_vec.x + dri_vec.y*dri_vec.y);
}

const double abs_sin_PI_X = std::abs(std::sin(CV_PI/90));

double lamd = 10;

double get_and_match(Mat& input_frame, Mat& input_template, Mat& input_template_feature, RotatedRect& getted_ammor, RotatedRect& getted_leap, Point2f &center_pt, double precision) {
	static Point2f srcTri[3];
	static Point2f dstTri[3];
	static Mat warp_mat(2, 3, CV_32FC1);

	static Mat dst_item_1 = input_template.clone(), dst_item_2 = input_template.clone();
	static Mat item1_feature, item2_feature;

	dstTri[0] = Point2f(0, 0);
	dstTri[1] = Point2f(input_template.cols - 1, 0);
	dstTri[2] = Point2f(0, input_template.rows - 1);

	static vector<vector<Point>> contours;
	static vector<Vec4i> hierarchy;
	static vector<Point> contour;

	static vector<Point2f> center_points_set;

	findContours(input_frame.clone(), contours, hierarchy, RETR_TREE, CHAIN_APPROX_NONE);

	static int contours_size;
	static int contour_size;

	static RotatedRect contour_rrect;
	static Point2f contour_rrect_points[4];

	static Mat diff_between_template_and_item1, diff_between_template_and_item2;
	static double diff_1_avr, diff_2_avr, input_template_points_num;
	static double now_diff, min_diff;

	input_template_points_num = (double)input_template.cols * (double)input_template.rows;
	min_diff = -1;

	static int i, j;
	static int getted_idx;

	j = 0;
	contours_size = contours.size();

	center_points_set.resize(0);

	for (i = 0; i < contours_size; i++) {
		contour = contours[i];
		contour_rrect = minAreaRect(contour);

		center_points_set.push_back(contour_rrect.center);

		if (hierarchy[i][2] < 0)continue;
		contour_rrect.points(contour_rrect_points);

		arrange_rrect_points((Point2f*)contour_rrect_points, (Point2f*)contour_rrect_points);

		srcTri[0] = contour_rrect_points[0];
		srcTri[1] = contour_rrect_points[1];
		srcTri[2] = contour_rrect_points[3];

		warp_mat = getAffineTransform(srcTri, dstTri);

		cv::warpAffine(input_frame, dst_item_1, warp_mat, dst_item_1.size());
		get_item_feature(dst_item_1, item1_feature);

		srcTri[0] = contour_rrect_points[2];
		srcTri[1] = contour_rrect_points[3];
		srcTri[2] = contour_rrect_points[1];

		warp_mat = getAffineTransform(srcTri, dstTri);

		cv::warpAffine(input_frame, dst_item_2, warp_mat, dst_item_2.size());
		get_item_feature(dst_item_2, item2_feature);

		absdiff(item1_feature, input_template_feature, diff_between_template_and_item1);
		absdiff(item2_feature, input_template_feature, diff_between_template_and_item2);

#if 0
		cv::imshow("template", input_template);
		cv::imshow("dst_item_1",dst_item_1);
		cv::imshow("dst_item_2",dst_item_2);
		cout << "diff_between_template_and_item1:" << diff_between_template_and_item1 << endl << endl;
		cout << "diff_between_template_and_item2:" << diff_between_template_and_item2 << endl << endl;
		cv::waitKey(0);
#endif
		diff_1_avr = (cv::sum(diff_between_template_and_item1))[0] / 2.0;
		diff_2_avr = (cv::sum(diff_between_template_and_item2))[0] / 2.0;
		now_diff = std::min(diff_1_avr, diff_2_avr);

		//cout << "now_diff: " << now_diff << endl;
		if (now_diff < precision && (j == 0 || now_diff < min_diff)) {
			min_diff = now_diff;
			getted_idx = i;
			j++;

			//cout << "item1_feature:" << item1_feature << endl;

#if GET_TEMPLATE
			cv::imshow("dst_item_1", dst_item_1);
			cv::imshow("dst_item_2", dst_item_2);
			if (cv::waitKey(0) == 27) {
				imwrite(program_root_path + "/../resources/dst_item_1.png", dst_item_1);
				imwrite(program_root_path + "/../resources/dst_item_2.png", dst_item_2);
				exit(0);
			}
#endif
		}

	}

	if(min_diff != -1)getted_ammor = minAreaRect(contours[hierarchy[getted_idx][2]]);
	else return min_diff;

	contour_rrect = minAreaRect(contours[getted_idx]);
	getted_leap = RotatedRect(contour_rrect);
	center_pt.x = -1; center_pt.y = -1;

	static double in_product, ex_product, r_vec_ammor_to_arm_dri, r_vec_ammor_to_point;
	static double cur_feature, now_best_feature;
	static Point2f vec_ammor_to_arm_dri, vec_ammor_to_point;

	j = 0;
	r_vec_ammor_to_arm_dri = find_ammor_to_arm_dri_vec(getted_ammor, contour_rrect.center, vec_ammor_to_arm_dri);

	for (i = 0; i < contours_size; i++) {
		if (i == getted_idx || i == hierarchy[getted_idx][2])continue;
		vec_ammor_to_point = center_points_set[i] - getted_ammor.center;

		r_vec_ammor_to_point = std::sqrt(vec_ammor_to_point.x * vec_ammor_to_point.x + vec_ammor_to_point.y * vec_ammor_to_point.y);

		in_product = vec_ammor_to_arm_dri.ddot(vec_ammor_to_point)/r_vec_ammor_to_arm_dri;
		ex_product = vec_ammor_to_arm_dri.cross(vec_ammor_to_point)/r_vec_ammor_to_arm_dri/r_vec_ammor_to_point;
		ex_product = std::abs(ex_product);
		
		cur_feature = in_product + ex_product;

		if (in_product > 0 && ex_product < abs_sin_PI_X && (j == 0 || cur_feature < now_best_feature) && in_product < 40.0 * r_vec_ammor_to_arm_dri) {
			now_best_feature = cur_feature;
			center_pt = center_points_set[i];
			j++;
		}
	}

	if(j==0)center_pt = lamd * vec_ammor_to_arm_dri + getted_ammor.center;

	return min_diff;

}

double get_and_match_AccMat(AccMat& input_frame_AccMat, AccMat& input_template_AccMat, Mat& input_template_feature, RotatedRect& getted_ammor, RotatedRect& getted_leap, Point2f &center_pt, double precision) {
	if(input_frame_AccMat.empty())return -1;

	static Point2f srcTri[3];
	static Point2f dstTri[3];
	Mat warp_mat(2, 3, CV_32FC1) ;

	AccMat dst_item_1 = AccMat() , dst_item_2 = AccMat();
	input_template_AccMat.copyTo(dst_item_1);
	dst_item_1.copyTo(dst_item_2);
	Mat item1_feature, item2_feature;

	dstTri[0] = Point2f(0, 0);
	dstTri[1] = Point2f(input_template_AccMat.cols - 1, 0);
	dstTri[2] = Point2f(0, input_template_AccMat.rows - 1);

	static vector<vector<Point>> contours;
	static vector<Vec4i> hierarchy;
	static vector<Point> contour;

	static vector<Point2f> center_points_set;

	findContours(input_frame_AccMat, contours, hierarchy, RETR_TREE, CHAIN_APPROX_NONE);

	static int contours_size;
	static int contour_size;

	static RotatedRect contour_rrect;
	static Point2f contour_rrect_points[4];

	AccMat diff_between_template_and_item1 = AccMat(), diff_between_template_and_item2 = AccMat();
	static double diff_1_avr, diff_2_avr, input_template_points_num;
	static double now_diff, min_diff;

	input_template_points_num = (double)input_template_AccMat.cols * (double)input_template_AccMat.rows;
	min_diff = -1;

	static int i, j;
	static int getted_idx;
	static int son_idx, son_num = 0;

	j = 0;
	contours_size = contours.size();

	center_points_set.resize(0);

	for (i = 0; i < contours_size; i++) {
		contour = contours[i];
		contour_rrect = minAreaRect(contour);

		center_points_set.push_back(contour_rrect.center);

		if (hierarchy[i][2] < 0)continue;
		contour_rrect.points(contour_rrect_points);

		son_idx = hierarchy[i][2];
		son_num = 0;

		while(son_idx > 0){
			son_num++;
			son_idx = hierarchy[son_idx][0];
		}

		if(son_num > 1)continue;

		arrange_rrect_points((Point2f*)contour_rrect_points, (Point2f*)contour_rrect_points);

		srcTri[0] = contour_rrect_points[0];
		srcTri[1] = contour_rrect_points[1];
		srcTri[2] = contour_rrect_points[3];

		warp_mat = getAffineTransform(srcTri, dstTri);

		cv::warpAffine(input_frame_AccMat, dst_item_1, warp_mat, dst_item_1.size());
		//if(dst_item_1.size().area() < 100)continue;
		get_item_feature_AccMat(dst_item_1, item1_feature, contour);

		srcTri[0] = contour_rrect_points[2];
		srcTri[1] = contour_rrect_points[3];
		srcTri[2] = contour_rrect_points[1];

		warp_mat = getAffineTransform(srcTri, dstTri);

		cv::warpAffine(input_frame_AccMat, dst_item_2, warp_mat, dst_item_2.size());
		if(dst_item_2.size().area() < 100)continue;
		get_item_feature_AccMat(dst_item_2, item2_feature, contour);

		//absdiff(item1_feature, input_template_feature_AccMat, diff_between_template_and_item1);
		//absdiff(item2_feature, input_template_feature_AccMat, diff_between_template_and_item2);

#if 0
		cv::imshow("template", input_template);
		cv::imshow("dst_item_1",dst_item_1);
		cv::imshow("dst_item_2",dst_item_2);
		cout << "diff_between_template_and_item1:" << diff_between_template_and_item1 << endl << endl;
		cout << "diff_between_template_and_item2:" << diff_between_template_and_item2 << endl << endl;
		cv::waitKey(0);
#endif
		//diff_1_avr = (cv::sum(diff_between_template_and_item1))[0] / 2.0;
		//diff_2_avr = (cv::sum(diff_between_template_and_item2))[0] / 2.0;
		now_diff = std::min(calc_err_H(item1_feature, input_template_feature), calc_err_H(item2_feature, input_template_feature));

		//cout << "now_diff: " << now_diff << endl;
		if (now_diff < precision && (j == 0 || now_diff < min_diff)) {
			min_diff = now_diff;
			getted_idx = i;
			j++;

			//cout << "item1_feature:" << item1_feature << endl;

#if GET_TEMPLATE
			cv::imshow("dst_item_1", dst_item_1);
			cv::imshow("dst_item_2", dst_item_2);
			if (cv::waitKey(0) == 27) {
				imwrite(program_root_path + "/../resources/dst_item_1.tif", dst_item_1);
				imwrite(program_root_path + "/../resources/dst_item_2.tif", dst_item_2);
				exit(0);
			}
#endif
		}

	}
/*
	dst_item_1.release();
	dst_item_2.release();
	item1_feature.release();
	item2_feature.release();
	input_template_feature_AccMat.release();
	input_frame_AccMat.release();
	diff_between_template_and_item1.release();
	diff_between_template_and_item2.release();
*/
	if(min_diff != -1)getted_ammor = minAreaRect(contours[hierarchy[getted_idx][2]]);
	else return min_diff;

	contour_rrect = minAreaRect(contours[getted_idx]);
	getted_leap = RotatedRect(contour_rrect);
	center_pt.x = -1; center_pt.y = -1;

	static double in_product, ex_product, r_vec_ammor_to_arm_dri, r_vec_ammor_to_point;
	static double cur_feature, now_best_feature;
	static Point2f vec_ammor_to_arm_dri, vec_ammor_to_point;

	j = 0;
	r_vec_ammor_to_arm_dri = find_ammor_to_arm_dri_vec(getted_ammor, contour_rrect.center, vec_ammor_to_arm_dri);

	for (i = 0; i < contours_size; i++) {
		if (i == getted_idx || i == hierarchy[getted_idx][2])continue;
		vec_ammor_to_point = center_points_set[i] - getted_ammor.center;

		r_vec_ammor_to_point = std::sqrt(vec_ammor_to_point.x * vec_ammor_to_point.x + vec_ammor_to_point.y * vec_ammor_to_point.y);

		in_product = vec_ammor_to_arm_dri.ddot(vec_ammor_to_point)/r_vec_ammor_to_arm_dri;
		ex_product = vec_ammor_to_arm_dri.cross(vec_ammor_to_point)/r_vec_ammor_to_arm_dri/r_vec_ammor_to_point;
		ex_product = std::abs(ex_product);
		
		cur_feature = in_product + ex_product;

		if (in_product > 0 && ex_product < abs_sin_PI_X && (j == 0 || cur_feature < now_best_feature) && in_product < 13.5 * r_vec_ammor_to_arm_dri) {
			now_best_feature = cur_feature;
			center_pt = center_points_set[i];
			j++;
		}
	}

	if(j==0)center_pt = lamd * vec_ammor_to_arm_dri + getted_ammor.center;

	return min_diff;

}

double position_proc(Point2f& io_point_angle, RotatedRect& ammor_rrect, Camera_INFO& camera_info, double uphead_angle){
	static float F_pix;
	static double x_distance, y_distance, z_distance1, z_distance2, z_distance;
	static float proj_ammor_height;
	static bool height_is_smaller = false;
	static float ammor_pix_height;
	static float ammor_rect_angle_rad = 0.0;
	
	static Point2f rrect_points[4], temp_point1, temp_point2;
	static Point2f point1_tan, point2_tan;
	static Point2f ammor_height_proj_vec;

	F_pix = (float)camera_info.F / (float)camera_info.pix_len;
	proj_ammor_height = std::cos((float)uphead_angle) * (float)AMMOR_HEIGHT;
	height_is_smaller = ammor_rrect.size.height < ammor_rrect.size.width;

	ammor_rrect.points(rrect_points);
	ammor_rect_angle_rad = ammor_rrect.angle * CV_PI / 180.0;

	if(height_is_smaller){
		ammor_pix_height = ammor_rrect.size.height;
		temp_point1 = (rrect_points[0] + rrect_points[1])/2.0;
		temp_point2 = (rrect_points[2] + rrect_points[3])/2.0;
		ammor_height_proj_vec.x = proj_ammor_height * std::sin(ammor_rect_angle_rad);
		ammor_height_proj_vec.y = proj_ammor_height * std::cos(ammor_rect_angle_rad);
	}else{
		ammor_pix_height = ammor_rrect.size.width;
		temp_point1 = (rrect_points[0] + rrect_points[3])/2.0;
		temp_point2 = (rrect_points[1] + rrect_points[2])/2.0;
		ammor_height_proj_vec.x = proj_ammor_height * std::cos(ammor_rect_angle_rad);
		ammor_height_proj_vec.y = proj_ammor_height * std::sin(ammor_rect_angle_rad);
	}

	point1_tan.x = temp_point1.x/F_pix;
	point1_tan.y = temp_point1.y/F_pix;
	point2_tan.x = ammor_rrect.center.x/F_pix;
	point2_tan.y = ammor_rrect.center.y/F_pix;

	z_distance1 = ammor_height_proj_vec.x / std::abs(point1_tan.x - point2_tan.x) * 0.5;
	z_distance1 += ammor_height_proj_vec.y / std::abs(point1_tan.y - point2_tan.y) * 0.5;
	z_distance1 /= 2.0;

	point1_tan.x = temp_point2.x/F_pix;
	point1_tan.y = temp_point2.y/F_pix;

	z_distance2 = ammor_height_proj_vec.x / std::abs(point1_tan.x - point2_tan.x) * 0.5;
	z_distance2 += ammor_height_proj_vec.y / std::abs(point1_tan.y - point2_tan.y) * 0.5;
	z_distance2 /= 2.0;

	z_distance = z_distance1 > z_distance2 ? z_distance1 : z_distance2;
	x_distance = z_distance * point2_tan.x;
	y_distance = z_distance * point2_tan.y;

	io_point_angle.x = std::atan(x_distance/(z_distance + camera_info.loc_hori_len_mm));
	io_point_angle.y = std::atan((y_distance + camera_info.loc_vert_len_mm)/(z_distance + camera_info.loc_hori_len_mm));

	io_point_angle.x = io_point_angle.x * 180.0 / CV_PI;
	io_point_angle.y = io_point_angle.y * 180.0 / CV_PI;

	return z_distance;
}
